﻿

using HalconDotNet;
using MotionRobot.Models;
using Prism.Commands;
using Prism.Services.Dialogs;
using System;
using System.Collections.ObjectModel;
using System.Threading.Tasks;
using System.Threading;
using System.Windows.Forms;
using ViewROI;
using MessageBox = System.Windows.Forms.MessageBox;
using LeaderCCSLaserUI.Services;
using LeaderCCSLaserUI.Models;
using Newtonsoft.Json;
using System.Linq;
using System.IO;
using NLog;
using System.Diagnostics;
using LeaderCCSLaserUI.Data;
using LeaderCCSLaserUI.Data.Extensions;
using System.Windows.Media;

namespace LeaderCCSLaserUI.ViewModels.Dialogs
{
    public class CameraCalibDialogViewModel : DialogViewModel
    {
        #region 变量
        private readonly IGTSCardService gTSCard;
        private readonly DrawROI drawROI;
        private CancellationTokenSource source0, source1;
        private Param systemParam;
        private static Logger logger = LogManager.GetCurrentClassLogger();

        private double mxYJogSpeed = 0, mzJogSpeed = 0, mz1SafePos = 0, mz2SafePos = 0;
        private int handIndex = 0; private string handDir = string.Empty;
        private readonly IHIKCameraService leftCamera;
        private readonly IHIKCameraService rightCamera;
        private bool isClosing = false;
        #endregion
        #region 属性
        private HImage cameraIamge0;
        public HImage CameraIamge0
        {
            get { return cameraIamge0; }
            set { SetProperty(ref cameraIamge0, value); }
        }
        private bool cameraRepaint0;
        public bool CameraRepaint0
        {
            get { return cameraRepaint0; }
            set { SetProperty(ref cameraRepaint0, value); }
        }
        private ObservableCollection<ROI> cameraROIList0 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList0
        {
            get { return cameraROIList0; }
            set { SetProperty(ref cameraROIList0, value); }
        }
        private HObject cameraAppendHObject0;
        public HObject CameraAppendHObject0
        {
            get { return cameraAppendHObject0; }
            set { SetProperty(ref cameraAppendHObject0, value); }
        }
        private HMsgEntry cameraAppendHMessage0;
        public HMsgEntry CameraAppendHMessage0
        {
            get { return cameraAppendHMessage0; }
            set { SetProperty(ref cameraAppendHMessage0, value); }
        }
        private Tuple<string, object> cameraGCStyle0;
        public Tuple<string, object> CameraGCStyle0
        {
            get { return cameraGCStyle0; }
            set { SetProperty(ref cameraGCStyle0, value); }
        }
        private bool isBusy;
        public bool IsBusy
        {
            get { return isBusy; }
            set { SetProperty(ref isBusy, value); }
        }
        private double cameraExposureTime;
        public double CameraExposureTime
        {
            get { return cameraExposureTime; }
            set { SetProperty(ref cameraExposureTime, value); }
        }
        private bool isDrawing;
        public bool IsDrawing
        {
            get { return isDrawing; }
            set { SetProperty(ref isDrawing, value); }
        }
        #endregion
        #region 命令
        private DelegateCommand<object> cameraOperateCommand;
        public DelegateCommand<object> CameraOperateCommand =>
            cameraOperateCommand ?? (cameraOperateCommand = new DelegateCommand<object>(ExecuteCameraOperateCommand));
        private DelegateCommand startCalibCommand;
        public DelegateCommand StartCalibCommand =>
            startCalibCommand ?? (startCalibCommand = new DelegateCommand(ExecuteStartCalibCommand));
        private DelegateCommand calcCalibCommand;
        public DelegateCommand CalcCalibCommand =>
            calcCalibCommand ?? (calcCalibCommand = new DelegateCommand(ExecuteCalcCalibCommand));
        private DelegateCommand createShapeModelCommand;
        public DelegateCommand CreateShapeModelCommand =>
            createShapeModelCommand ?? (createShapeModelCommand = new DelegateCommand(ExecuteCreateShapeModelCommand));
        private DelegateCommand findShapeModelCommand;
        public DelegateCommand FindShapeModelCommand =>
            findShapeModelCommand ?? (findShapeModelCommand = new DelegateCommand(ExecuteFindShapeModelCommand));
        private DelegateCommand<object> liveGrabCommand;
        public DelegateCommand<object> LiveGrabCommand =>
            liveGrabCommand ?? (liveGrabCommand = new DelegateCommand<object>(ExecuteLiveGrabCommand));
        void ExecuteLiveGrabCommand(object isLiveGrab)
        {
            if ((bool)isLiveGrab)
            {
                source1 = new CancellationTokenSource();
                CancellationToken token = source1.Token;
                Task.Run(() => ContinueGrab(token), token);
                IsBusy = true;
            }
            else
            {
                if (source1 != null)
                {
                    source1.Cancel();
                }
                IsBusy = false;
            }
        }
        void ExecuteFindShapeModelCommand()
        {
            if (CameraIamge0 != null)
            {
                try
                {
                    string dirPath = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $"Camera\\Calib\\{handDir}");
                    HTuple ModelID;
                    HOperatorSet.ReadShapeModel(System.IO.Path.Combine(System.Environment.CurrentDirectory, dirPath, $"CalibShapeModelID.shm"), out ModelID);
                    HTuple Row, Column, Angle, Score;
                    HOperatorSet.FindShapeModel(CameraIamge0, ModelID, new HTuple(-180).TupleRad(), new HTuple(360).TupleRad(), 0.5, 1, 0.5, "least_squares", new HTuple(5).TupleConcat(1), 0.75, out Row, out Column, out Angle, out Score);
                    HOperatorSet.ClearShapeModel(ModelID);
                    if (Score.TupleLength() > 0)
                    {
                        HTuple HomMat2D;
                        HOperatorSet.HomMat2dIdentity(out HomMat2D);
                        HOperatorSet.HomMat2dRotate(HomMat2D, Angle, 0, 0, out HomMat2D);
                        HOperatorSet.HomMat2dTranslate(HomMat2D, Row, Column, out HomMat2D);
                        HTuple hv_DxfStatus; HObject ModelContours;
                        HOperatorSet.ReadContourXldDxf(out ModelContours, System.IO.Path.Combine(dirPath, $"CalibShapeModelContours.dxf"), new HTuple(), new HTuple(), out hv_DxfStatus);
                        HObject TransContours;
                        HOperatorSet.AffineTransContourXld(ModelContours, out TransContours, HomMat2D);
                        CameraAppendHObject0 = null;
                        CameraGCStyle0 = new Tuple<string, object>("Color", "green");
                        CameraGCStyle0 = new Tuple<string, object>("DrawMode", "margin");
                        CameraAppendHObject0 = TransContours;
                        CameraAppendHMessage0 = null;
                        CameraAppendHMessage0 = new HMsgEntry($"坐标:{Row.D:F1},{Column.D:F1},{Angle.TupleDeg().D:F1}° 分数:{Score.D:F1}", 100, 100);
                    }
                    else
                    {
                        MessageBox.Show("未来找到模板", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
                    }
                }
                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message, "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
                }
            }
            else
            {
                MessageBox.Show("图像不能为空", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }
        }
        void ExecuteCreateShapeModelCommand()
        {
            if (CameraIamge0 != null)
            {
                if (MessageBox.Show("确认要创建模板吗？", "确认", MessageBoxButtons.YesNo, MessageBoxIcon.Question, MessageBoxDefaultButton.Button2) == System.Windows.Forms.DialogResult.Yes)
                {
                    string dirPath = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $"Camera\\Calib\\{handDir}");
                    try
                    {
                        IsDrawing = true;
                        var roi = drawROI.imageViewer.DrawROI(ROI.ROI_TYPE_REGION);
                        IsDrawing = false;
                        if (!isClosing)
                        {
                            HRegion ModelRegion = roi.getRegion();
                            HObject TemplateImage;
                            HOperatorSet.ReduceDomain(CameraIamge0, ModelRegion, out TemplateImage);
                            HTuple ModelID;
                            HOperatorSet.CreateShapeModel(TemplateImage, 3, new HTuple(-180).TupleRad(), new HTuple(360).TupleRad(), new HTuple(1).TupleRad(), new HTuple("none").TupleConcat("no_pregeneration"), "use_polarity", new HTuple(30).TupleConcat(30).TupleConcat(0), 10, out ModelID);
                            HObject ModelContours;
                            HOperatorSet.GetShapeModelContours(out ModelContours, ModelID, 1);
                            HTuple ModelRegionArea, RefRow, RefColumn;
                            HOperatorSet.AreaCenter(ModelRegion, out ModelRegionArea, out RefRow, out RefColumn);
                            HTuple HomMat2D;
                            HOperatorSet.VectorAngleToRigid(0, 0, 0, RefRow, RefColumn, 0, out HomMat2D);
                            HObject TransContours;
                            HOperatorSet.AffineTransContourXld(ModelContours, out TransContours, HomMat2D);
                            CameraAppendHObject0 = null;
                            CameraGCStyle0 = new Tuple<string, object>("Color", "green");
                            CameraGCStyle0 = new Tuple<string, object>("DrawMode", "margin");
                            CameraAppendHObject0 = TransContours;
                            CameraAppendHMessage0 = null;
                            CameraAppendHMessage0 = new HMsgEntry($"坐标:{RefRow.D:F1},{RefColumn.D:F1}", 100, 100);
                            HOperatorSet.WriteShapeModel(ModelID, System.IO.Path.Combine(dirPath, "CalibShapeModelID.shm"));
                            HOperatorSet.ClearShapeModel(ModelID);
                            HOperatorSet.WriteContourXldDxf(ModelContours, System.IO.Path.Combine(dirPath, "CalibShapeModelContours.dxf"));
                        }                          
                    }
                    catch (Exception ex)
                    {
                        MessageBox.Show(ex.Message, "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
                    }
                }
            }
            else
            {
                MessageBox.Show("图像不能为空", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }
        }
        void ExecuteCalcCalibCommand()
        {
            if (CameraIamge0 != null)
            {
                if (MessageBox.Show("确认要计算标定吗？", "确认", MessageBoxButtons.YesNo, MessageBoxIcon.Question, MessageBoxDefaultButton.Button2) == System.Windows.Forms.DialogResult.Yes)
                {
                    try
                    {
                        string dirPath = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $"Camera\\Calib\\{handDir}");
                        string filepath_image = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $"Camera\\Calib\\{handDir}\\images");
                        double dist;
                        MPoint calibPoint;
                        switch (handIndex)
                        {
                            case 0:
                                calibPoint = systemParam.LeftCameraCalibLazer;
                                dist = systemParam.LeftCameraCalibDist;
                                break;
                            default:
                                calibPoint = systemParam.RightCameraCalibLazer;
                                dist = systemParam.RightCameraCalibDist;
                                break;
                        }
                        double[][] diffArray = new double[9][]
                       { new double[]{ 0,0,0},
                                     new double[]{ dist * 1, 0,0} ,
                                     new double[]{ dist * 1, dist * 1,0},
                                     new double[]{ 0, dist * 1,0},
                                     new double[]{ dist * -1, dist * 1,0},
                                     new double[]{ dist * -1,0,0},
                                     new double[]{ dist * -1, dist * -1,0},
                                     new double[]{ 0, dist * -1,0},
                                     new double[]{ dist * 1, dist * -1,0}};
                        double[][] Array1 = new double[9][];
                        CameraAppendHObject0 = null;
                        CameraAppendHMessage0 = null;
                        CameraGCStyle0 = new Tuple<string, object>("Color", "green");
                        CameraGCStyle0 = new Tuple<string, object>("DrawMode", "margin");
                        HOperatorSet.ReadShapeModel(System.IO.Path.Combine(dirPath, $"CalibShapeModelID.shm"), out var ModelID);
                        HOperatorSet.ReadContourXldDxf(out var ModelContours, System.IO.Path.Combine(dirPath, $"CalibShapeModelContours.dxf"), new HTuple(), new HTuple(), out var hv_DxfStatus);
                        for (int i = 0; i < 9; i++)
                        {
                            try
                            {
                                HObject img;
                                HOperatorSet.ReadImage(out img, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath_image, $"CalibImage{i + 1}.jpg"));
                                HTuple Row, Column, Angle, Score;
                                HOperatorSet.FindShapeModel(img, ModelID, new HTuple(-180).TupleRad(), new HTuple(360).TupleRad(), 0.5, 1, 0.5, "least_squares", new HTuple(5).TupleConcat(1), 0.75, out Row, out Column, out Angle, out Score);

                                HTuple HomMat2D;
                                HOperatorSet.HomMat2dIdentity(out HomMat2D);
                                HOperatorSet.HomMat2dRotate(HomMat2D, Angle, 0, 0, out HomMat2D);
                                HOperatorSet.HomMat2dTranslate(HomMat2D, Row, Column, out HomMat2D);

                                HObject TransContours;
                                HOperatorSet.AffineTransContourXld(ModelContours, out TransContours, HomMat2D);


                                Array1[i] = new double[4] { Row.D, Column.D, calibPoint.X + diffArray[i][0] * -1, calibPoint.Y + diffArray[i][1] * -1 };//这里是相机移动了，而产品没有动。

                                CameraAppendHMessage0 = new HMsgEntry($"{i + 1}", (int)Row.D, (int)Column.D);

                                CameraAppendHObject0 = TransContours;
                            }
                            catch (Exception ex)
                            {
                                Debug.WriteLine(ex.Message);
                                Array1[i] = new double[4] { 0, 0, calibPoint.X + diffArray[i][0] * -1, calibPoint.Y + diffArray[i][1] * -1 };
                            }
                        }

                        HOperatorSet.VectorToHomMat2d(new HTuple(Array1[0][0]).TupleConcat(Array1[1][0]).TupleConcat(Array1[2][0]).TupleConcat(Array1[3][0]).TupleConcat(Array1[4][0]).TupleConcat(Array1[5][0]).TupleConcat(Array1[6][0]).TupleConcat(Array1[7][0]).TupleConcat(Array1[8][0]),
                                                        new HTuple(Array1[0][1]).TupleConcat(Array1[1][1]).TupleConcat(Array1[2][1]).TupleConcat(Array1[3][1]).TupleConcat(Array1[4][1]).TupleConcat(Array1[5][1]).TupleConcat(Array1[6][1]).TupleConcat(Array1[7][1]).TupleConcat(Array1[8][1]),
                                                        new HTuple(Array1[0][2]).TupleConcat(Array1[1][2]).TupleConcat(Array1[2][2]).TupleConcat(Array1[3][2]).TupleConcat(Array1[4][2]).TupleConcat(Array1[5][2]).TupleConcat(Array1[6][2]).TupleConcat(Array1[7][2]).TupleConcat(Array1[8][2]),
                                                        new HTuple(Array1[0][3]).TupleConcat(Array1[1][3]).TupleConcat(Array1[2][3]).TupleConcat(Array1[3][3]).TupleConcat(Array1[4][3]).TupleConcat(Array1[5][3]).TupleConcat(Array1[6][3]).TupleConcat(Array1[7][3]).TupleConcat(Array1[8][3])
                                                        , out var homMat2D);

                        HOperatorSet.WriteTuple(homMat2D, System.IO.Path.Combine(dirPath, "CalibHomMat2D.tup"));

                        HOperatorSet.VectorToHomMat2d(new HTuple(Array1[0][2]).TupleConcat(Array1[1][2]).TupleConcat(Array1[2][2]).TupleConcat(Array1[3][2]).TupleConcat(Array1[4][2]).TupleConcat(Array1[5][2]).TupleConcat(Array1[6][2]).TupleConcat(Array1[7][2]).TupleConcat(Array1[8][2]),
                                                  new HTuple(Array1[0][3]).TupleConcat(Array1[1][3]).TupleConcat(Array1[2][3]).TupleConcat(Array1[3][3]).TupleConcat(Array1[4][3]).TupleConcat(Array1[5][3]).TupleConcat(Array1[6][3]).TupleConcat(Array1[7][3]).TupleConcat(Array1[8][3]),
                                                  new HTuple(Array1[0][0]).TupleConcat(Array1[1][0]).TupleConcat(Array1[2][0]).TupleConcat(Array1[3][0]).TupleConcat(Array1[4][0]).TupleConcat(Array1[5][0]).TupleConcat(Array1[6][0]).TupleConcat(Array1[7][0]).TupleConcat(Array1[8][0]),
                                                  new HTuple(Array1[0][1]).TupleConcat(Array1[1][1]).TupleConcat(Array1[2][1]).TupleConcat(Array1[3][1]).TupleConcat(Array1[4][1]).TupleConcat(Array1[5][1]).TupleConcat(Array1[6][1]).TupleConcat(Array1[7][1]).TupleConcat(Array1[8][1])
                                                  , out var homMat2D1);

                        HOperatorSet.WriteTuple(homMat2D1, System.IO.Path.Combine(dirPath, "CalibHomMat2D_Rev.tup"));
                        aggregator.SendMessage("ReloadCameraParam", "Run");
                    }
                    catch (Exception ex)
                    {
                        MessageBox.Show(ex.Message, "错误", MessageBoxButtons.OK);
                    }
                }
            }
            else
            {
                MessageBox.Show("图像不能为空", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }
        }
        async void ExecuteStartCalibCommand()
        {
            if (MessageBox.Show("确认要执行标定动作吗？", "确认", MessageBoxButtons.YesNo, MessageBoxIcon.Question, MessageBoxDefaultButton.Button2) == System.Windows.Forms.DialogResult.Yes)
            {
                IsBusy = true;
                AxisParm axisX, axisY, axisZ;
                double zsafePos;
                MPoint calibPoint;
                double calibDist;
                IHIKCameraService cameraService;
                switch (handIndex)
                {
                    case 0:
                        axisX = gTSCard.A1;
                        axisY = gTSCard.A2;
                        axisZ = gTSCard.A3;
                        calibPoint = systemParam.LeftCameraCalib;
                        zsafePos = mz1SafePos;
                        calibDist = systemParam.LeftCameraCalibDist;
                        cameraService = leftCamera;
                        break;
                    default:
                        axisX = gTSCard.A5;
                        axisY = gTSCard.A6;
                        axisZ = gTSCard.A7;
                        calibPoint = systemParam.RightCameraCalib;
                        zsafePos = mz2SafePos;
                        calibDist = systemParam.RightCameraCalibDist;
                        cameraService = rightCamera;
                        break;
                }
                GTSCard.ServoOn(axisX);//X
                GTSCard.ServoOn(axisY);//Y
                GTSCard.ServoOn(axisZ);//Z
                GTSCard.AxisPosSet(axisX, GTSCard.GetEnc(axisX));
                GTSCard.AxisPosSet(axisY, GTSCard.GetEnc(axisY));
                GTSCard.AxisPosSet(axisZ, GTSCard.GetEnc(axisZ));
                source0 = new CancellationTokenSource();
                CancellationToken token = source0.Token;
                await Task.Run(() =>
                {
                    CalibMoveAction(token, cameraService, calibPoint, calibDist, axisX, axisY, axisZ, zsafePos);
                }, token).ContinueWith(t => { IsBusy = false; });
            }
        }
        async void ExecuteCameraOperateCommand(object obj)
        {
            if (obj is string str)
            {
                switch (str)
                {
                    case "打开":
                        {
                            OpenFileDialog openFileDialog = new OpenFileDialog();
                            openFileDialog.Filter = "Image Files|*.png;*.bmp;*.jpg;*.tif";
                            if (openFileDialog.ShowDialog() == System.Windows.Forms.DialogResult.OK)
                            {
                                HObject image;
                                HOperatorSet.ReadImage(out image, openFileDialog.FileName);
                                CameraIamge0 = new HImage(image);
                            }
                        }
                        break;
                    case "拍照":
                        {
                            IHIKCameraService cameraService = handIndex == 0 ? leftCamera : rightCamera;
                            cameraService.ImageQueue.Clear();
                            cameraService.SetTriggerMode(0);
                            cameraService.SetExposureTime((float)CameraExposureTime);
                            cameraService.StartGrabe();
                            IsBusy = true;
                            await Task.Run(() => {
                                int mIndex = 0;
                                while (mIndex < 1)
                                {
                                    HObject? hObj;
                                    if (cameraService.ImageQueue.TryPeek(out hObj))
                                    {
                                        cameraService.ImageQueue.TryDequeue(out hObj);
                                        CameraIamge0 = new HImage(hObj);
                                        mIndex++;
                                    }
                                }
                            }).ContinueWith(t => { cameraService.StopGrabe(); IsBusy = false; });
                        }
                        break;
                    default:
                        break;
                }
            }
        }
        #endregion
        #region 导航
        public override void OnDialogClosed()
        {
            if (source0 != null)
            {
                source0.Cancel();
            }
            if (source1 != null)
            {
                source1.Cancel();
            }
            IHIKCameraService cameraService = handIndex == 0 ? leftCamera : rightCamera;
            cameraService.StopGrabe();
            aggregator.SendMessage("ReloadCameraParam", "Run");
            isClosing = true;
            if (IsDrawing)
            {
                MFunc.HIOCancelDraw();
            }
        }
        public override void OnDialogOpened(IDialogParameters parameters)
        {
            handIndex = parameters.GetValue<int>("HandIndex");

            switch (handIndex)
            {
                case 0:
                    Title = "左相机-标定";
                    handDir = "Left";
                    break;
                case 1:
                    Title = "右相机-标定";
                    handDir = "Right";
                    break;
                default:
                    break;
            }
            LoadParam();
            LoadParamFromDB();
            // Get the subdirectories for the specified directory.
            DirectoryInfo dir = new DirectoryInfo(Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $"Camera\\Calib\\{handDir}"));
            if (!dir.Exists)
            {
                // If the destination directory doesn't exist, create it.       
                Directory.CreateDirectory(Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $"Camera\\Calib\\{handDir}"));
            }
        }
        #endregion
        #region 构造函数
        public CameraCalibDialogViewModel(IContainerProvider containerProvider) : base(containerProvider)
        {
            gTSCard = containerProvider.Resolve<IGTSCardService>("GTSCard");
            drawROI = containerProvider.Resolve<DrawROI>();
            leftCamera = containerProvider.Resolve<IHIKCameraService>("LeftCamera");
            rightCamera = containerProvider.Resolve<IHIKCameraService>("RightCamera");
            IsBusy = false;
            aggregator.ResgiterMessage(arg =>
            {
                switch (arg.Message)
                {
                    case "EMG":
                        {
                            if (source0 != null)
                            {
                                source0.Cancel();
                            }
                        }
                        break;
                    default:
                        break;
                }
            }, "Run");
        }
        #endregion
        #region 功能函数
        private void ContinueGrab(CancellationToken token)
        {
            IHIKCameraService cameraService = handIndex == 0 ? leftCamera : rightCamera;
            cameraService.ImageQueue.Clear();
            cameraService.SetTriggerMode(0);
            cameraService.SetExposureTime((float)CameraExposureTime);
            cameraService.StartGrabe();
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    cameraService.StopGrabe();
                    return;
                }
                HObject? hObj;
                if (cameraService.ImageQueue.TryPeek(out hObj))
                {
                    cameraService.ImageQueue.TryDequeue(out hObj);
                    CameraIamge0 = new HImage(hObj);
                }
                System.Threading.Thread.Sleep(1);
            }
        }
        void LoadParam()
        {
            try
            {
                //Json序列化，从文件读取
                string jsonString = File.ReadAllText(System.IO.Path.Combine(System.AppDomain.CurrentDomain.BaseDirectory, "Param.json"));
                systemParam = JsonConvert.DeserializeObject<Param>(jsonString);
            }
            catch (Exception ex)
            {
                logger.Error(ex);
            }
        }
        void LoadParamFromDB()
        {
            try
            {
                using var mdb = new MDbContext(systemParam.PartList[systemParam.PartIndex]);
                var _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "XYJogSpeed");
                if (_mParam != null)
                {
                    mxYJogSpeed = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZJogSpeed");
                if (_mParam != null)
                {
                    mzJogSpeed = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZSafePos" && m.Hand == 0);
                if (_mParam != null)
                {
                    mz1SafePos = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "ZSafePos" && m.Hand == 1);
                if (_mParam != null)
                {
                    mz2SafePos = double.Parse(_mParam.Value);
                }
                _mParam = mdb.MParams.FirstOrDefault(m => m.Name == "CameraExposureTime" && m.Hand == handIndex);
                if (_mParam != null)
                {
                    CameraExposureTime = double.Parse(_mParam.Value);
                }
            }
            catch (Exception ex)
            {
                logger.Error(ex);
            }
        }
        private void CalibMoveAction(CancellationToken token, IHIKCameraService hIKCameraService, MPoint calibPoint, double dist, AxisParm axisX, AxisParm axisY, AxisParm axisZ, double zSafe)
        {
            Stopwatch sw = new Stopwatch();
            string dirPath = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, $"Camera\\Calib\\{handDir}\\images");
            try
            {
                double[][] diffArray = new double[9][]
                              { new double[]{ 0,0,0},
                                     new double[]{ dist * 1, 0,0} ,
                                     new double[]{ dist * 1, dist * 1,0},
                                     new double[]{ 0, dist * 1,0},
                                     new double[]{ dist * -1, dist * 1,0},
                                     new double[]{ dist * -1,0,0},
                                     new double[]{ dist * -1, dist * -1,0},
                                     new double[]{ 0, dist * -1,0},
                                     new double[]{ dist * 1, dist * -1,0}};
                int stepnum = 1;
                int moveIndex = 0;
                while (true)
                {
                    if (token.IsCancellationRequested)
                    {
                        return;
                    }
                    switch (stepnum)
                    {
                        case 1:
                            GTSCard.AxisPosMove(ref axisZ, zSafe, mzJogSpeed);
                            stepnum = 2;
                            break;
                        case 2:
                            if (GTSCard.AxisPosMoveCheckDone(axisZ))
                            {
                                stepnum = 0;
                            }
                            break;
                        case 0:
                            if (moveIndex >= 9)
                            {
                                stepnum = 100;
                            }
                            else
                            {
                                stepnum = 3;
                            }
                            break;

                        case 3:
                            GTSCard.AxisPosMove(ref axisX, calibPoint.X + diffArray[moveIndex][0], mxYJogSpeed);
                            GTSCard.AxisPosMove(ref axisY, calibPoint.Y + diffArray[moveIndex][1], mxYJogSpeed);
                            GTSCard.AxisPosMove(ref axisZ, calibPoint.Z, mzJogSpeed);
                            stepnum = 4;
                            break;
                        case 4:
                            if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY) && GTSCard.AxisPosMoveCheckDone(axisZ))
                            {
                                moveIndex++;
                                sw.Restart();
                                stepnum = 7;
                            }
                            break;
                        case 7:
                            if (sw.Elapsed.TotalSeconds > 0.2)
                            {
                                sw.Stop();
                                hIKCameraService.ImageQueue.Clear();
                                hIKCameraService.SetExposureTime((float)CameraExposureTime);
                                hIKCameraService.SetTriggerMode(0);
                                hIKCameraService.StartGrabe();
                                stepnum = 8;
                            }
                            break;
                        case 8:
                            if (hIKCameraService.ImageQueue.Count > 0)
                            {
                                hIKCameraService.StopGrabe();
                                hIKCameraService.ImageQueue.TryDequeue(out var hObj);
                                CameraIamge0 = new HImage(hObj);
                                Directory.CreateDirectory(dirPath);
                                HOperatorSet.WriteImage(hObj, "jpeg", 0, System.IO.Path.Combine(dirPath, $"CalibImage{moveIndex}.jpg"));
                                stepnum = 0;
                            }
                            break;

                        case 100:
                            GTSCard.AxisPosMove(ref axisZ, zSafe, mzJogSpeed);
                            stepnum = 101;
                            break;
                        case 101:
                            if (GTSCard.AxisPosMoveCheckDone(axisZ))
                            {
                                return;
                            }
                            break;
                        default:
                            break;
                    }
                    System.Threading.Thread.Sleep(100);
                }
            }
            catch (Exception ex)
            {
                logger.Error(ex);
            }
        }

        #endregion
    }
}
